#include "Communication/SubCliffSensor.h"
#include "modules/CtrlModule/ActivePlanner/CliffFollowPlanner.h"
#include "data_struct/base/DataSet.h"

namespace behavior_controller
{
CliffData DataSet::cliff_data;
namespace SubCliffSensor
{
static rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr cliffSubSim;
static rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr cliffSub;

void sub_callback2(const std_msgs::msg::UInt8MultiArray::SharedPtr msg)
{
    DataSet::cliff_data.SetAllData(msg->data, CLIFF_MAXDIST);
}

void sub_callback(const sensor_msgs::msg::Range::SharedPtr msg)
{
    DataSet::cliff_data.SetData(DataSet::cliff_data.cliff_map[msg->header.frame_id], msg->range, CLIFF_MAXDIST);
}

void SubCliffSensorInfo(rclcpp::Node::SharedPtr &node_handle)
{
    if (SIM_MODE == 0)
    {
        cliffSub = node_handle->create_subscription<std_msgs::msg::UInt8MultiArray>(
                "cliff_sensor", 1, sub_callback2);
    } else
    {
        cliffSubSim = node_handle->create_subscription<sensor_msgs::msg::Range>(
                "cliff_sensor", 2, sub_callback);
    }

    std::cout << "SubCliffSensor SubCliffSensorInfo" << std::endl;
}
} // namespace SubCliffSensor

} // namespace behavior_controller